/**
@author Daniel Sidlauskas Miller

Source code for flight control support functions
*/

#include "twi_master_driver.h"
#include <stdlib.h>
#include "avr_compiler.h"
#include "support.h"
#include <stdio.h>
#include "stddef.h"
#include <avr/io.h>

void uart_spi_init(void){
	//TX enable
	USARTD0.CTRLB = 0b00001000;
	//MASTER SPI mode
	USARTD0.CTRLC = 0b11000010;

	//2 megabauld (clock divded by 4 minux 1)
	USARTD0.BAUDCTRLA = 2;
	USARTD0.BAUDCTRLB = 0;
}




void twiInitiate(TWI_Master_t *title,TWI_t *interface){
	TWI_MasterInit(title, interface, TWI_MASTER_INTLVL_LO_gc, TWI_BAUDSETTING);
	interface->MASTER.CTRLB |= 0x04;
}
		

void CCPWrite( volatile uint8_t * address, uint8_t value )
{
	#if defined __GNUC__
	AVR_ENTER_CRITICAL_REGION( );
	volatile uint8_t * tmpAddr = address;
	#ifdef RAMPZ
	RAMPZ = 0;
	#endif
	asm volatile(
		"movw r30,  %0"      "\n\t"
		"ldi  r16,  %2"      "\n\t"
		"out   %3, r16"      "\n\t"
		"st     Z,  %1"       "\n\t"
		:
		: "r" (tmpAddr), "r" (value), "M" (CCP_IOREG_gc), "i" (&CCP)
		: "r16", "r30", "r31"
	);

	AVR_LEAVE_CRITICAL_REGION( );
	#endif
}

void clock_32mhz_init(void){
	OSC.CTRL = 0b00000010;//Enable oscilator
	NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc;
//	DFLLRC32M.CALA = (pgm_read_byte(offsetof(NVM_PROD_SIGNATURES_t, USBRCOSC))) & 0xff;
	NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc;
//	DFLLRC32M.CALB = (pgm_read_byte(offsetof(NVM_PROD_SIGNATURES_t, USBRCOSC))) >> 8 ;


	while(!(OSC.STATUS & 0b00000010));//wait for it to be ready
	CCPWrite(&CLK.CTRL, 0b00000001);//Enable oscilator
}

void clock_48mhz_init(void){
		OSC.PLLCTRL = 0b10100011;
	OSC.CTRL |= OSC_PLLEN_bm;

	//Get Clock callibration bytes
	NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc;
	//DFLLRC32M.CALA = (pgm_read_byte(offsetof(NVM_PROD_SIGNATURES_t, USBRCOSC))) & 0xff;
	NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc;
	//DFLLRC32M.CALB = (pgm_read_byte(offsetof(NVM_PROD_SIGNATURES_t, USBRCOSC))) >> 8 ;

	CCPWrite(&CLK.CTRL, 0b00000100);//Enable oscilator
	OSC.CTRL = 0b00000010;//Enable oscilator
	while(!(OSC.STATUS & 0b00000010));//wait for it to be ready

}

void clock_12mhz_external_init(void){
	//enable external oscilator
	OSC.XOSCCTRL = 0b10001011;
	OSC.CTRL = (1<<3);
	//wait for it to be ready
	while(!(OSC.STATUS & 0b00001000));
	CCPWrite(&CLK.CTRL, 0b00000011);//Enable oscilator
}
	
void trigger_estop(uint8_t * estop_flag){
	PORTA.OUT |= (1<<7) | (3);
	PORTD.OUT &= ~0b00100000;
	*estop_flag = 1;
}
